#!/usr/bin/env python3
# -*- coding: utf-8 -*-
"""
机器人立柱控制和测距模块
包含Robotlift立柱控制类和Tof测距类
"""

import time
import threading
import logging
import sys
import select
import tty
import termios
from EMM42_CTL import Emm42_CAN_Controller

# 导入PH2501测距模块
try:
    import os
    sys.path.append(os.path.join(os.path.dirname(__file__), '..', '..', 'Sensor', 'PSH2501'))
    from PH2501 import PSH2501_DToF
    PH2501_AVAILABLE = True
    print("✅ PH2501测距模块导入成功")
except ImportError as e:
    print(f"⚠️ PH2501测距模块导入失败: {e}")
    PH2501_AVAILABLE = False

# 导入STP23L测距模块
try:
    from STP23L import STP23L_RangeFinder
    STP23L_AVAILABLE = True
    print("✅ STP23L测距模块导入成功")
except ImportError as e:
    print(f"⚠️ STP23L测距模块导入失败: {e}")
    STP23L_AVAILABLE = False

# 全局变量声明（与主程序保持一致）
pillar_distance = 0.0  # 立柱电机距离（米）
ranging_active = False  # 测距功能是否激活
stp23l_distance = 0.0  # STP23L测距距离（米）
stp23l_ranging_active = False  # STP23L测距功能是否激活


class Robotlift:
    """立柱电机封装：基于EMM42驱动，统一速度/停止/解除堵转/测速接口"""

    def __init__(self, emm42_ctl: Emm42_CAN_Controller = None, channel_handle=None, device_id: int = 1):
        self.logger = logging.getLogger(__name__)
        self.speed_limit_rpm = 100
        self.emm42 = None
        try:
            if emm42_ctl is not None:
                self.emm42 = emm42_ctl
            elif channel_handle is not None:
                self.emm42 = Emm42_CAN_Controller(channel_handle=channel_handle, device_id=device_id)
                # 使能电机
                try:
                    self.emm42.motor_enable(enable=True)
                except Exception:
                    pass
        except Exception as e:
            self.logger.warning(f"Robotlift初始化失败: {e}")
            self.emm42 = None

        if self.emm42 is None:
            self.logger.warning("Robotlift未绑定EMM42控制器，功能受限")

    def is_ready(self) -> bool:
        return self.emm42 is not None

    def set_speed(self, direction: int, speed_rpm: int, accel_level: int = 0x00, sync_flag: int = 0x00) -> bool:
        """速度模式控制（限速0~100 rpm）"""
        if not self.is_ready():
            self.logger.error("EMM42未初始化，无法设置立柱速度")
            return False
        try:
            # 限速保护：0~100 rpm
            clamped_speed = max(0, min(int(speed_rpm), self.speed_limit_rpm))
            return self.emm42.speed_control(direction=direction, speed_rpm=clamped_speed,
                                            accel_level=accel_level, sync_flag=sync_flag)
        except Exception as e:
            self.logger.error(f"设置立柱速度失败: {e}")
            return False

    def immediate_stop(self, sync_flag: int = 0x00) -> bool:
        """立即停止"""
        if not self.is_ready():
            self.logger.error("EMM42未初始化，无法立即停止")
            return False
        try:
            return self.emm42.immediate_stop(sync_flag=sync_flag)
        except Exception as e:
            self.logger.error(f"立柱立即停止失败: {e}")
            return False

    def clear_lock_protection(self) -> bool:
        """解除堵转保护"""
        if not self.is_ready():
            self.logger.error("EMM42未初始化，无法解除堵转保护")
            return False
        try:
            return self.emm42.clear_lock_protection()
        except Exception as e:
            self.logger.error(f"解除堵转保护失败: {e}")
            return False

    def read_real_speed(self):
        """读取实时转速与方向"""
        if not self.is_ready():
            return None
        try:
            return self.emm42.read_real_speed()
        except Exception:
            return None

    def Liftcontroller(self):
        def up(speed):
            self.set_speed(direction=0, speed_rpm=speed)
        def down(speed):
            self.set_speed(direction=1, speed_rpm=speed)
        def stop():
            self.immediate_stop()


class Tof:
    """ToF测距封装类：解耦PH2501与STP23L的使用，并维护原有全局变量更新"""

    def __init__(self):
        self.logger = logging.getLogger(__name__)

        # PH2501测距模块
        self.psh2501 = None
        self.ranging_active = False
        self.ranging_thread = None
        self.ranging_monitoring = False
        self.ranging_lock = threading.Lock()
        self.pillar_distance = 0.0

        if PH2501_AVAILABLE:
            try:
                self.psh2501 = PSH2501_DToF()
                if self.psh2501.open_serial():
                    print("✅ PH2501测距模块初始化成功")
                    self.logger.info("PH2501测距模块初始化成功")
                else:
                    print("⚠️ PH2501测距模块串口打开失败")
                    self.logger.warning("PH2501测距模块串口打开失败")
                    self.psh2501 = None
            except Exception as e:
                print(f"⚠️ PH2501测距模块初始化失败: {e}")
                self.logger.warning(f"PH2501测距模块初始化失败: {e}")
                self.psh2501 = None
        else:
            print("⚠️ PH2501测距模块不可用")
            self.logger.warning("PH2501测距模块不可用")

        # STP23L测距模块
        self.stp23l = None
        self.stp23l_ranging_active = False
        self.stp23l_ranging_thread = None
        self.stp23l_ranging_monitoring = False
        self.stp23l_ranging_lock = threading.Lock()
        self.stp23l_distance = 0.0

        if STP23L_AVAILABLE:
            try:
                self.stp23l = STP23L_RangeFinder()
                print("✅ STP23L测距模块初始化成功")
                self.logger.info("STP23L测距模块初始化成功")
            except Exception as e:
                print(f"⚠️ STP23L测距模块初始化失败: {e}")
                self.logger.warning(f"STP23L测距模块初始化失败: {e}")
                self.stp23l = None
        else:
            print("⚠️ STP23L测距模块不可用")
            self.logger.warning("STP23L测距模块不可用")

    def _ranging_thread(self):
        """PH2501测距线程 - 持续读取PH2501测距数据"""
        self.logger.info("测距线程启动")

        ranging_interval = 0.1  # 100ms测距间隔

        try:
            while self.ranging_monitoring:
                start_time = time.time()

                # 检查测距是否激活
                with self.ranging_lock:
                    if not self.ranging_active:
                        time.sleep(ranging_interval)
                        continue

                # 检查PH2501模块是否可用
                if not self.psh2501:
                    time.sleep(ranging_interval)
                    continue

                try:
                    # 读取测距数据（56字节）
                    data = self.psh2501.ser.read(56)
                    if len(data) == 56:
                        # 解析并显示距离数据
                        result = self.psh2501.parse_open_flow_reply(data)
                        if result:
                            # 更新全局测距变量
                            global pillar_distance
                            with self.ranging_lock:
                                pillar_distance = result['distance_m']
                                self.pillar_distance = result['distance_m']
                            # 记录到日志（避免过于频繁）
                            if hasattr(self, '_last_ranging_log') and time.time() - self._last_ranging_log > 2.0:
                                self.logger.info(f"测距数据: {result['distance_m']:.4f}m, 信噪比: {result['snr']}")
                                self._last_ranging_log = time.time()
                            elif not hasattr(self, '_last_ranging_log'):
                                self._last_ranging_log = time.time()
                    else:
                        if len(data) > 0:
                            self.logger.warning(f"接收数据长度异常：期望56字节，实际{len(data)}字节")
                except Exception as e:
                    self.logger.error(f"读取测距数据异常：{str(e)}")

                # 控制周期等待
                elapsed_time = time.time() - start_time
                sleep_time = max(0, ranging_interval - elapsed_time)
                time.sleep(sleep_time)

        except Exception as e:
            self.logger.error(f"测距线程错误: {e}")

        self.logger.info("测距线程结束")

    def start_ranging(self):
        """启动PH2501测距功能"""
        if not self.psh2501:
            print("⚠️ PH2501测距模块不可用，无法启动测距")
            return False

        if self.ranging_monitoring:
            self.logger.warning("测距线程已在运行中")
            return True

        # 发送开流指令
        try:
            open_reply = self.psh2501.send_command("open_flow")
            if not open_reply:
                print("❌ 发送开流指令失败")
                return False

            result = self.psh2501.parse_open_flow_reply(open_reply)
            if not result:
                print("❌ 解析开流指令回复失败")
                return False

            print("✅ PH2501测距模块开流成功")
            self.logger.info("PH2501测距模块开流成功")
        except Exception as e:
            print(f"❌ PH2501测距模块开流失败: {e}")
            self.logger.error(f"PH2501测距模块开流失败: {e}")
            return False

        # 启动测距线程
        with self.ranging_lock:
            self.ranging_active = True

        self.ranging_monitoring = True
        self.ranging_thread = threading.Thread(
            target=self._ranging_thread,
            daemon=True
        )
        self.ranging_thread.start()
        self.logger.info("测距线程已启动")

        # 更新全局变量
        global ranging_active
        ranging_active = True
        return True

    def stop_ranging(self):
        """停止PH2501测距功能"""
        if not self.psh2501:
            return False

        # 停止测距线程
        with self.ranging_lock:
            self.ranging_active = False

        self.ranging_monitoring = False
        if self.ranging_thread:
            self.ranging_thread.join(timeout=1)

        # 发送关流指令
        try:
            close_reply = self.psh2501.send_command("close_flow")
            if close_reply:
                result = self.psh2501.parse_close_flow_reply(close_reply)
                if result is True:
                    print("✅ PH2501测距模块关流成功")
                    self.logger.info("PH2501测距模块关流成功")
                else:
                    print("⚠️ PH2501测距模块关流失败")
                    self.logger.warning("PH2501测距模块关流失败")
            else:
                print("⚠️ 发送关流指令失败")
                self.logger.warning("发送关流指令失败")
        except Exception as e:
            print(f"⚠️ PH2501测距模块关流异常: {e}")
            self.logger.error(f"PH2501测距模块关流异常: {e}")

        # 更新全局变量
        global ranging_active
        ranging_active = False

        return True

    def toggle_ranging(self):
        """切换PH2501测距功能开关"""
        global ranging_active
        # 使用Tof自身状态进行判断
        if self.ranging_active:
            # 当前激活，则关闭
            if self.stop_ranging():
                print("🔴 测距功能已关闭")
                return False
            else:
                print("⚠️ 关闭测距功能失败")
                return self.ranging_active
        else:
            # 当前关闭，则打开
            if self.start_ranging():
                print("🟢 测距功能已开启")
                return True
            else:
                print("⚠️ 开启测距功能失败")
                return self.ranging_active

    def _stp23l_ranging_thread(self):
        """STP23L测距线程 - 持续读取STP23L测距数据"""
        self.logger.info("STP23L测距线程启动")

        try:
            while self.stp23l_ranging_monitoring:
                # 检查测距是否激活
                with self.stp23l_ranging_lock:
                    if not self.stp23l_ranging_active:
                        time.sleep(0.1)
                        continue

                # 检查STP23L模块是否可用
                if not self.stp23l:
                    time.sleep(0.1)
                    continue

                try:
                    # 获取STP23L测距数据
                    distance = self.stp23l.get_distance()

                    # 更新全局测距变量
                    global stp23l_distance
                    with self.stp23l_ranging_lock:
                        stp23l_distance = distance
                        self.stp23l_distance = distance

                    # 记录到日志（避免过于频繁）
                    if hasattr(self, '_last_stp23l_ranging_log') and time.time() - self._last_stp23l_ranging_log > 2.0:
                        self.logger.info(f"STP23L测距数据: {distance:.4f}m")
                        self._last_stp23l_ranging_log = time.time()
                    elif not hasattr(self, '_last_stp23l_ranging_log'):
                        self._last_stp23l_ranging_log = time.time()

                except Exception as e:
                    self.logger.error(f"读取STP23L测距数据异常：{str(e)}")

                time.sleep(0.1)  # 控制读取频率

        except Exception as e:
            self.logger.error(f"STP23L测距线程错误: {e}")

        self.logger.info("STP23L测距线程结束")

    def start_stp23l_ranging(self):
        """启动STP23L测距功能"""
        if not self.stp23l:
            print("⚠️ STP23L测距模块不可用，无法启动测距")
            return False

        if self.stp23l_ranging_monitoring:
            self.logger.warning("STP23L测距线程已在运行中")
            return True

        # 启动STP23L测距
        if not self.stp23l.start_ranging():
            print("❌ STP23L测距模块启动失败")
            return False

        print("✅ STP23L测距模块启动成功")
        self.logger.info("STP23L测距模块启动成功")

        # 启动测距线程
        with self.stp23l_ranging_lock:
            self.stp23l_ranging_active = True

        self.stp23l_ranging_monitoring = True
        self.stp23l_ranging_thread = threading.Thread(
            target=self._stp23l_ranging_thread,
            daemon=True
        )
        self.stp23l_ranging_thread.start()
        self.logger.info("STP23L测距线程已启动")

        # 更新全局变量
        global stp23l_ranging_active
        stp23l_ranging_active = True

        return True

    def stop_stp23l_ranging(self):
        """停止STP23L测距功能"""
        if not self.stp23l:
            return False

        # 停止测距线程
        with self.stp23l_ranging_lock:
            self.stp23l_ranging_active = False

        self.stp23l_ranging_monitoring = False
        if self.stp23l_ranging_thread:
            self.stp23l_ranging_thread.join(timeout=1)

        # 停止STP23L测距
        if self.stp23l.stop_ranging():
            print("✅ STP23L测距模块停止成功")
            self.logger.info("STP23L测距模块停止成功")
        else:
            print("⚠️ STP23L测距模块停止失败")
            self.logger.warning("STP23L测距模块停止失败")

        # 更新全局变量
        global stp23l_ranging_active
        stp23l_ranging_active = False

        return True

    def toggle_stp23l_ranging(self):
        """切换STP23L测距功能开关"""
        global stp23l_ranging_active
        # 使用Tof自身状态进行判断
        if self.stp23l_ranging_active:
            # 当前激活，则关闭
            if self.stop_stp23l_ranging():
                print("🔴 STP23L测距功能已关闭")
                return False
            else:
                print("⚠️ 关闭STP23L测距功能失败")
                return self.stp23l_ranging_active
        else:
            # 当前关闭，则打开
            if self.start_stp23l_ranging():
                print("🟢 STP23L测距功能已开启")
                return True
            else:
                print("⚠️ 开启STP23L测距功能失败")
                return self.stp23l_ranging_active